#include <buola/scene/transform/crigidbody.h>
#include <buola/scene/cgeode.h>
#include <btBulletDynamicsCommon.h>

namespace buola { namespace scene {

CRigidBody::CRigidBody(PGeode pGeode, double pMass,const mat::CMat34d &pPos)
    :   mGeode(pGeode)
{
    mLocalTransform=pPos;
    mCollisionShape=pGeode->GetCollisionShape();
    btVector3 lInertia;
    mCollisionShape->calculateLocalInertia(pMass,lInertia);
    btRigidBody::btRigidBodyConstructionInfo lInfo(pMass,this,mCollisionShape.get(),lInertia);
//    lInfo.m_rollingFriction=0.3;
    lInfo.m_restitution=0;
    lInfo.m_angularDamping=1;
    lInfo.m_linearDamping=0.75;
    lInfo.m_friction=1.0;
    mBody=PBulletRigidBody(new btRigidBody(lInfo));
    AddObject(pGeode);
}

CRigidBody::~CRigidBody()
{
}

void CRigidBody::CalcLocalTransform()
{
    mLocalTransformValid=true;
}

void CRigidBody::getWorldTransform(btTransform &pT) const
{
    pT.setBasis(btMatrix3x3(mLocalTransform(0,0),mLocalTransform(0,1),mLocalTransform(0,2),
                            mLocalTransform(1,0),mLocalTransform(1,1),mLocalTransform(1,2),
                            mLocalTransform(2,0),mLocalTransform(2,1),mLocalTransform(2,2)));
    pT.setOrigin(btVector3(mLocalTransform(0,3),mLocalTransform(1,3),mLocalTransform(2,3)));
}

void CRigidBody::setWorldTransform(const btTransform &pT)
{
    const auto &lBasis=pT.getBasis();
    const auto &lOrigin=pT.getOrigin();
    
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
            mLocalTransform(i,j)=lBasis[i][j];
        mLocalTransform(i,3)=lOrigin[i];
    }
}

/*namespace scene*/ } /*namespace buola*/ }
